﻿#pragma once
#include "rec/robotino/com/all.h"
#include "rec/core_lt/utils.h"
#include "rec/robotino/com/DistanceSensor.h"
#include "rec/core_lt/Timer.h"

using namespace rec::robotino::com;

//#include "Robotino/Robotino_Camera_Interface.h"
//#include "Robotino/Vertex.h"
#include "Robotino/MyCamera.h"
#include "Robotino/Lines_Coords.h"
#include "Robotino/Blind_PathFinding_Plane.h"
//#include "Robotino/BariersLevel.h"
#include "Robotino/Phisix_Location.h"
#include "Robotino/Path.h"
#include "Robotino/Lines_Coords.h"

#include "Robotino/algorithms.h"

#include "DirectX/Direct_All.h"
#include <rec\robotino\com\Bumper.h>
#include <rec\robotino\com\Odometry.h>

#define MAX_SPEED_FROM_USER 1800
#define MAX_SPEED_FROM_AA 500
#define M_PI 3.141592654


class Robotino3D : public DirectX, public Com
{
private:

//! ------------------------------------------------Private Data Base-------------------------------------------------*/
	OmniDrive				omniDrive;
	MyCamera				Cam;
	Physix_Location			Lasers;

	LPDIRECT3DTEXTURE9		time_texture1,
							time_texture2,
							time_texture3;

	D3DXVECTOR2				dir_lastPath;

	D3DXQUATERNION			Robo_Move;

	Lines_Coords			Lines;
	Path					SearchPath;

	HWND					hWnd;
	WCHAR					buf[512];

	time_t					timer_move_only_lasers,
							timer_lost_lasers;

	float					Robo_Cam_Alpha,
							Robo_Cam_Rx,
							Robo_Cam_Height;
		
	float					x,
							y,
							r,
							Speed_Auto_Move;

	float					xMove,
							yMove,
							rMove;

	bool					Post_R_Move_Left;

	Bumper                  bumper;
	Odometry                odometry;
	
	rec::core_lt::Timer		timer;
	
	bool				isDoingSmthng;
	bool				isSecTime;
	int					cur_lost_pos;
	
	
//! -----------------------------------------------------Luser data---------------------------------------------------*/
	//
	//
	//
	//


//! --------------------------------------------------Private Metods--------------------------------------------------*/
	void Robo_Move_Normalize()
	{
		D3DXVECTOR2			Vec2;
		D3DXVec2Normalize	(&Vec2, &D3DXVECTOR2(Robo_Move.x, Robo_Move.y));
		Robo_Move.x		=	Vec2.x;
		Robo_Move.y		=	Vec2.y;
	}
	void Robo_Move_SpeedActivate()
	{
		Robo_Move.x	*=	Robo_Move.w;
		Robo_Move.y	*=	Robo_Move.w;
		Robo_Move.z *=	Robo_Move.w;
	}
	bool User_KeyboardScanner()
	{
		Robo_Move.x		=	Robo_Move.y	= Robo_Move.z = 0;
		
		bool	robo_move	=	false;

		if(keyboard	[ DIK_W			]	&0x80)		{Robo_Move.x = 1.0f;	robo_move	=	true;}
		if(keyboard	[ DIK_A			]	&0x80)		{Robo_Move.y = 1.0f;	robo_move	=	true;}
		if(keyboard	[ DIK_S			]	&0x80)		{Robo_Move.x = -1.0f;	robo_move	=	true;}
		if(keyboard	[ DIK_D			]	&0x80)		{Robo_Move.y = -1.0f;	robo_move	=	true;}
		if(keyboard	[ DIK_Q			]	&0x80)		{Robo_Move.z = 1.0f;	robo_move	=	true;}
		if(keyboard	[ DIK_E			]	&0x80)		{Robo_Move.z = -1.0f;	robo_move	=	true;}

		if(keyboard	[ DIK_L			]	&0x80)		{Robo_Move.w -= 1.0f;	robo_move	=	true;}
		if(keyboard	[ DIK_P			]	&0x80)		{Robo_Move.w += 1.0f;	robo_move	=	true;}

		if(Robo_Move.w < 0)						Robo_Move.w = 0;
		if(Robo_Move.w > MAX_SPEED_FROM_USER)	Robo_Move.w = MAX_SPEED_FROM_USER;

		Robo_Move_Normalize();
		Robo_Move_SpeedActivate();

		if(robo_move)
			setVelocity(Robo_Move.y, Robo_Move.x, Robo_Move.z, Robo_Move.w);


		if(keyboard [ DIK_RETURN	]	&0x80)		{/*Move_to_line();*/}
		if(keyboard [ DIK_UP		]	&0x80)		{/*Robo_Cam_Rx += 0.1f; Cam.SetupVB(pd3dDevice, 240, 320, Robo_Cam_Height, Robo_Cam_Alpha, Robo_Cam_Rx);*/}
		if(keyboard [ DIK_DOWN		]	&0x80)		{/*Robo_Cam_Rx -= 0.1f; Cam.SetupVB(pd3dDevice, 240, 320, Robo_Cam_Height, Robo_Cam_Alpha, Robo_Cam_Rx);*/}
		if(keyboard [ DIK_LEFT		]	&0x80)		{}
		if(keyboard [ DIK_RIGHT		]	&0x80)		{}

		if(keyboard [ DIK_PRIOR		]	&0x80)		{/*Cam.WorldRotatePlusX();*/}
		if(keyboard [ DIK_NEXT		]	&0x80)		{/*Cam.WorldRotateMinusX();*/}

		if(keyboard [ DIK_MINUS		]	&0x80)		{/*Cam.CameraMoveMinusZ();*/}
		if(keyboard [ DIK_ADD		]	&0x80)		{/*Cam.CameraMovePlusZ();*/}

		return robo_move;
	}

	void setVelocityForRot( float x, float y, float r )
	{
		omniDrive.setVelocity( x, y, r );
	}

	void setVelocity(float x, float y, float r, float speedAutoMove) 
	{
		if ( !x && !y && !r )
		{
			omniDrive.setVelocity( 0, 0, 0 );
			return;
		}
		D3DXVECTOR2			Vec2(x, y);
		D3DXVec2Normalize	(&Vec2, &Vec2);

		x		=	Vec2.x;
		y		=	Vec2.y;

		x *= speedAutoMove;
		y *= speedAutoMove;
		r *= speedAutoMove;

		xMove = (xMove * 3 + x) / 4;
		yMove = (yMove * 3 + y) / 4;
		rMove = (rMove * 3 + r) / 4;

		omniDrive.setVelocity(yMove, xMove, rMove / 1000);
	}

	void Set_Radius_Point_Location_from_Texture(float R)
	{
		Lines.set_Radius(R);
	}
	float Get_Radius_Point_Location_from_Texture()
	{
		return Lines.get_Radius();
	}
	void Set_Filter_ColorTexture(int r,int g, int b)
	{
		unsigned char		*data = new unsigned char[4];


		LPDIRECT3DSURFACE9	buffer;
		D3DLOCKED_RECT		lock;

		StaticTextures[0]->GetSurfaceLevel(0, &buffer);
		buffer->LockRect(&lock, NULL, D3DLOCK_DISCARD);

		data = (unsigned char*) lock.pBits;
		data[0] = b;
		data[1] = g;
		data[2] = r;
		data[3] = 255;
		

		buffer->UnlockRect();

		//delete data;		
	}

	//! --------------------------------------------------Events Functions------------------------------------------------*/
	bool f_b_Connect()
	{
		if(!isConnected())
		{
			LPDIRECT3DTEXTURE9		buf_textura		=	NULL,
									buf_textura2	=	NULL,
									buf_textura3	=	NULL;
			D3DXCreateTexture(pd3dDevice, 512, 256, 1, D3DUSAGE_RENDERTARGET,	D3DFMT_A8R8G8B8, D3DPOOL_DEFAULT, &buf_textura);
			D3DXCreateTexture(pd3dDevice, 512, 256, 1, D3DUSAGE_DYNAMIC,		D3DFMT_A8R8G8B8, D3DPOOL_DEFAULT, &buf_textura2);
			D3DXCreateTexture(pd3dDevice, 512, 256, 1, D3DUSAGE_DYNAMIC,		D3DFMT_A8R8G8B8, D3DPOOL_DEFAULT, &buf_textura3);

			/*D3DXCreateTexture(pd3dDevice, 640, 472, 1, D3DUSAGE_RENDERTARGET,	D3DFMT_A8R8G8B8, D3DPOOL_DEFAULT, &buf_textura);
			D3DXCreateTexture(pd3dDevice, 640, 472, 1, D3DUSAGE_DYNAMIC,		D3DFMT_A8R8G8B8, D3DPOOL_DEFAULT, &buf_textura2);
			D3DXCreateTexture(pd3dDevice, 640, 472, 1, D3DUSAGE_DYNAMIC,		D3DFMT_A8R8G8B8, D3DPOOL_DEFAULT, &buf_textura3);*/

			time_texture1	=	StaticTextures[1];
			time_texture2	=	StaticTextures[2];
			time_texture3	=	StaticTextures[3];

			StaticTextures[2]	=	buf_textura2;
			StaticTextures[3]	=	buf_textura;
			StaticTextures[4]	=	buf_textura3;

			//Rectangles.		push_back			(*new D3DRectangle(32.0f,	22.5f,	-0.1f,	92.0f,	-22.5f,		-0.1f,	-0.1f,	-0.1f));
			//StaticTextures.	push_back			(buf_textura);

			//Rectangles.		push_back			(*new D3DRectangle(-30.0f,  22.5f,	-0.1f,	30.0f,	-22.5f,		-0.1f,	-0.1f,	-0.1f));
			//StaticTextures.	push_back			(buf_textura2);

			//Rectangles.		push_back			(*new D3DRectangle(-92.0f,  22.5f,	-0.1f,	-32.0f,	-22.5f,		-0.1f,	-0.1f,	-0.1f));
			//StaticTextures.	push_back			(buf_textura3);

			Cam.			SetSurfaceMap		(StaticTextures[StaticTextures.size()-2]);
			Cam.			SetSurfaceTrack		(StaticTextures[StaticTextures.size()-1]);
			Cam.			SetupVB				(pd3dDevice, 240, 320, Robo_Cam_Height, Robo_Cam_Alpha, Robo_Cam_Rx);
			Cam.			CreateTexture		(pd3dDevice);

			StaticTextures[StaticTextures.size()-3] = Cam.GetTexture();

			bumper.setComId( id() );
			odometry.setComId( id() );
				
			//SetupVB();

			//init("5.63.74.125:8080");	// Hamachi notebook
			//init("5.33.43.46:8080");	// Hamachi PC
			// init("127.0.0.1:8080");		// NoteBook sim
			init("172.26.201.2");		// Robotino
		}
		else
		{
			//Rectangles.erase(--Rectangles.end());
			//StaticTextures.erase(--StaticTextures.end());
			//SetupVB();
			//NOT WORKED!!!!!!!!!!!!!
			StaticTextures[1]	=	time_texture1;
			StaticTextures[2]	=	time_texture2;
			StaticTextures[3]	=	time_texture3;
		}
		//Sleep(200);

		return true;
	}
	bool f_b_Line_REAL()
	{
		if(Lines.isLock())	
			return false;


		//Lines.Locked();

		//Coords  *down = new Coords(SearchPath.getPathPoint());
		Lines.Locked();
			Vertex down = Vertex(Lines.getDownPoint());
			int index = Lines.getDownPointIndex();
		Lines.Unlock();
		//Coords *next = new Coords(Lines.getCoords()->at(Lines.getDownPointIndex()+2));

		//if(down->Position.x != 0 && down->Position.y != 0)
		//{

		//}

		float	x_	=	x,
				y_	=	y,
				r_	=	r;

		bool	flag_active	=	false;

		Vertex *thertyPoint = Lines.getCoords(2);


		D3DXVECTOR2 dir(0, 0);

		if(thertyPoint != NULL)
		{

			dir.x = thertyPoint->Position.x - down.Position.x;
			dir.y = thertyPoint->Position.y - down.Position.y;
			
			D3DXVec2Normalize(&dir, &dir);

			flag_active = true;
			
			x_ = (32 - down.Position.x) * Cam.getHeight_Real() / 150;
			y_ = (dir.y * 2) * (dir.y * 2) * (dir.y * 5) * (dir.y * 2) * (dir.y * 2) * (dir.y * 5);
			r_ = dir.x * 200;
		}


		//Lines.Unlock();

		moveToLineFinish(x_, y_, r_, flag_active);

		setVelocity(x, y, -r * 2.5f, Speed_Auto_Move);

		Sleep(50);

		return flag_active;
	}

	void moveToLineFinish(float &x_, float &y_, float &r_, bool flag_active)
	{
		x = (x_ + x * 3)		/ 4;
		y = (y_ * 2 + y * 3)	/ 5;
		r = (r_ + r * 2)		/ 3;

		bool isLasersMove = moveOnLasers(x, y, r, false);
		
		if(isLasersMove)
			labyrinth(Speed_Auto_Move);
		
		if(!flag_active)
		{
			y_	=	(dir_lastPath.y * 2 + yMove) / 60; // TODO: Here need correct constants
			r_	=	-dir_lastPath.x * Speed_Auto_Move * 50;
		}
		else {
			dir_lastPath.x = x;
			dir_lastPath.y = y;
		}
	}

	bool f_b_Line()
	{
		Lines.Locked();
			//Vertex down = *new Vertex(Lines.getNEWDownPoint());
			Vertex down = Vertex(Lines.getDownPoint());
		Lines.Unlock();
		if(down == Vertex())
			return false;

		if(down.Dir_children.x == -1 && down.Dir_children.y == -1)
			return false;

		float	x_	=	0,
				y_	=	0.5f * Speed_Auto_Move,//600 * Speed_Auto_Move,
				r_	=	0;

		bool	flag_active	=	false;

		if(down.length_children != 0)
		{
			flag_active = true;
			
			x_ = (32 - down.Position.x) * Cam.getHeight_Real() / 100;
			y_ = (down.Dir_children.y * 2) * (down.Dir_children.y * 2) * (down.Dir_children.y * 5) * (down.Dir_children.y * 2) * (down.Dir_children.y * 2) * (down.Dir_children.y * 5);
			r_ = down.Dir_children.x * 900;
		}

		/*if(down->length_father != 0)
		{
			flag_active = true;

			x = (32 - down->Position.x) * Cam.getHeight_Real() / 100;
			y = (down->Dir_father.y * 2) * (down->Dir_father.y * 2) * (down->Dir_father.y * 5) * (down->Dir_father.y * 2) * (down->Dir_father.y * 2) * (down->Dir_father.y * 5);
			r = down->Dir_father.x * 900;
		}
*/
		
		
		moveToLineFinish(x_, y_, r_, flag_active);

		setVelocity(x, y, -r, Speed_Auto_Move);
		// omniDrive.setVelocity(y * Speed_Auto_Move, x * Speed_Auto_Move, -r * (Speed_Auto_Move/5));

		Sleep(50);

		return flag_active;
	}

	bool moveOnLasers(float &x, float &y, float &r, bool isNeedMove) 
	{
		bool				laser_detected = false;

		for(int q=0; q<Lasers.buf.size(); q++)
		{
			if(Lasers.Detected(q))
			{
				laser_detected	=	true;
				D3DXVECTOR2		dir_buf;

				dir_buf.x -= Lasers.getDirs()->at(q).x;
				dir_buf.y -= Lasers.getDirs()->at(q).y;

				D3DXVec2Normalize(&dir_buf, &dir_buf);

				Lasers.Bariers.lengths[q] = 1;
			}
		}
			
		if(!laser_detected)
			timer_move_only_lasers = clock();
		else
		{
			if(isNeedMove)
			{
				D3DXVECTOR3		Path_ = Lasers.Bariers.GetPath(timer_move_only_lasers, D3DXVECTOR3(dir_lastPath.x, dir_lastPath.y, r));
		
				x	=	Path_.x;
				y	=	Path_.y;
				r	=	Path_.z;
			
				setVelocity(Path_.x, Path_.y, -Path_.z, Speed_Auto_Move);
				// omniDrive.setVelocity(Path_.y * Speed_Auto_Move,  Path_.x * Speed_Auto_Move, -Path_.z * (Speed_Auto_Move/5));
			}
			Sleep(50);
			timer_lost_lasers = clock();
			return true;
		}

		if(clock() - timer_lost_lasers > 3000)
			Lasers.Bariers.Nulled();
		else
			return true;		

		return laser_detected;
	}


	bool f_v_Update_Speed()
	{
		((D3DXScrollBar*)UI[2])->CursorMove();
		Robo_Move.w = ((D3DXScrollBar*)UI[2])->getCursor_position() * MAX_SPEED_FROM_USER;
		((D3DXScrollBar*)UI[2])->UpdateCursor(pd3dDevice);
		return true;
	}
	bool f_v_Update_Alpha()
	{
		((D3DXScrollBar*)UI[3])->CursorMove();
		Robo_Cam_Rx = ((D3DXScrollBar*)UI[3])->getCursor_position() * 90;

		((D3DXScrollBar*)UI[3])->UpdateCursor(pd3dDevice);

		Cam.SetupVB(pd3dDevice, 240, 320, Robo_Cam_Height, Robo_Cam_Alpha, Robo_Cam_Rx);

		return true;
	}
	bool f_v_Update_Height()
	{
		((D3DXScrollBar*)UI[4])->CursorMove();
		Robo_Cam_Height = ((D3DXScrollBar*)UI[4])->getCursor_position() * 5000;

		((D3DXScrollBar*)UI[4])->UpdateCursor(pd3dDevice);

		Cam.SetupVB(pd3dDevice, 240, 320, Robo_Cam_Height, Robo_Cam_Alpha, Robo_Cam_Rx);

		return true;
	}
	bool f_v_Filter_R()
	{
		((D3DXScrollBar*)UI[5])->CursorMove();
		Cam.Color_Filter.x = ((D3DXScrollBar*)UI[5])->getCursor_position() * 255;

		((D3DXScrollBar*)UI[5])->UpdateCursor(pd3dDevice);

		Set_Filter_ColorTexture(Cam.Color_Filter.x, Cam.Color_Filter.y, Cam.Color_Filter.z);

		return true;
	}
	bool f_v_Filter_G()
	{
		((D3DXScrollBar*)UI[6])->CursorMove();
		Cam.Color_Filter.y = ((D3DXScrollBar*)UI[6])->getCursor_position() * 255;

		((D3DXScrollBar*)UI[6])->UpdateCursor(pd3dDevice);

		Set_Filter_ColorTexture(Cam.Color_Filter.x, Cam.Color_Filter.y, Cam.Color_Filter.z);

		return true;
	}
	bool f_v_Filter_B()
	{
		((D3DXScrollBar*)UI[7])->CursorMove();
		Cam.Color_Filter.z = ((D3DXScrollBar*)UI[7])->getCursor_position() * 255;

		((D3DXScrollBar*)UI[7])->UpdateCursor(pd3dDevice);

		Set_Filter_ColorTexture(Cam.Color_Filter.x, Cam.Color_Filter.y, Cam.Color_Filter.z);

		return true;
	}
	bool f_v_Filter_Size()
	{
		((D3DXScrollBar*)UI[8])->CursorMove();
		Cam.Color_Filter.w = ((D3DXScrollBar*)UI[8])->getCursor_position() * 510;

		((D3DXScrollBar*)UI[8])->UpdateCursor(pd3dDevice);

		return true;
	}
	bool f_v_SpeedRegulator()
	{
		((D3DXScrollBar*)UI[9])->CursorMove();
		Speed_Auto_Move = ((D3DXScrollBar*)UI[9])->getCursor_position() * MAX_SPEED_FROM_AA;
		
		// Robo_Move.w = ((D3DXScrollBar*)UI[2])->getCursor_position() * MAX_SPEED_FROM_AA;

		((D3DXScrollBar*)UI[9])->UpdateCursor(pd3dDevice);

		return true;
	}

	static bool static_f_b_Connect(LPVOID Object)
	{
		return ((Robotino3D*)Object)->f_b_Connect();
	}
	static bool static_f_b_Line(LPVOID Object)
	{
		
		// return ((Robotino3D*)Object)->goDistance( 1, 150 );
		
		return ((Robotino3D*)Object)->f_b_Line_REAL();
		// return ((Robotino3D*)Object)->labyrinth( 300 );
		//return ((Robotino3D*)Object)->arc( 150, 50, 270, 2 );
		//return ((Robotino3D*)Object)->labyrinthLostWall( 250 );
		//return ((Robotino3D*)Object)->rotate( 60.0, 90.0 );
		//return ((Robotino3D*)Object)->goSide( 300, 50 );
		//return ((Robotino3D*)Object)->goDistance( 300, 50 );	
	}
	static bool static_f_v_Update_Speed(LPVOID Object)
	{
		return ((Robotino3D*)Object)->f_v_Update_Speed();
	}
	static bool static_f_v_Update_Alpha(LPVOID Object)
	{
		return ((Robotino3D*)Object)->f_v_Update_Alpha();
	}
	static bool static_f_v_Update_Height(LPVOID Object)
	{
		return ((Robotino3D*)Object)->f_v_Update_Height();
	}
	static bool static_f_v_Filter_R(LPVOID Object)
	{
		return ((Robotino3D*)Object)->f_v_Filter_R(); 
	}
	static bool static_f_v_Filter_G(LPVOID Object)
	{
		return ((Robotino3D*)Object)->f_v_Filter_G(); 
	}
	static bool static_f_v_Filter_B(LPVOID Object)
	{
		return ((Robotino3D*)Object)->f_v_Filter_B(); 
	}
	static bool static_f_v_Filter_Size(LPVOID Object)
	{
		return ((Robotino3D*)Object)->f_v_Filter_Size();
	}
	static bool static_f_v_SpeedRegulator(LPVOID Object)
	{
		return ((Robotino3D*)Object)->f_v_SpeedRegulator();
	}


public:
	Robotino3D() : DirectX() , Com()	{}
	Robotino3D(HWND hWnd_,
		HINSTANCE hInst,
		bool Windowed,
		bool Enable_Super_Light,
		bool Light_Enable, 
		UINT BackBufferCount, 
		UINT MultiSampleType, 
		UINT TextDetal, 
		UINT MaxAnisotropi) : DirectX(hWnd_, 
									hInst,
									Windowed,
									Enable_Super_Light,
									Light_Enable, 
									BackBufferCount, 
									MultiSampleType, 
									TextDetal, 
									MaxAnisotropi), Com()
	{
		Robo_Move.x		=	Robo_Move.y	= Robo_Move.z = Robo_Move.w = 0;	
		hWnd			=	hWnd_;
		Robo_Cam_Alpha	=	0;
		Robo_Cam_Rx		=	0;
		Robo_Cam_Height =	0;

		x				=	0;
		y				=	100;
		r				=	0;

		xMove			=	0;
		yMove			=	0;
		rMove			=	0;

		Speed_Auto_Move =	0;
		timer			=	rec::core_lt::Timer();

		Post_R_Move_Left=	false;
		isDoingSmthng	=	false;
		isSecTime		=	false;
		cur_lost_pos	=	0;
	}
	void Setup_Real_World_Location(float Real_Cam_Height_, float Real_Cam_Alpha_, float Real_Cam_Rx_)
	{
		Robo_Cam_Alpha	=	Real_Cam_Alpha_;
		Robo_Cam_Rx		=	Real_Cam_Rx_;
		Robo_Cam_Height	=	Real_Cam_Height_;
	}
	void Set_Radius_Detecdetlines(int Radius)
	{
		Lines.set_Radius(Radius);
	}

//! -------------------------------------------------Visual Functions-------------------------------------------------*/
	void Setup_Robotino_Interface()
	{
		SetMatrixWorld(0.0f,0,0);

		Lasers	=	*new Physix_Location(pd3dDevice, D3DXVECTOR3(-4, 40, -12));
		Lasers.setSensativity(1.3f);

		Rectangles.clear();					// x1		y1		z1		x2		y2			z2		z3		z4
		Rectangles.push_back(*new D3DRectangle(40.0f,   60.0f,  -1.0f,  88.0f,  50.0f,      -1.0f,  -1.0f,  -1.0f));
		Rectangles.push_back(*new D3DRectangle(-700.0f, 384.0f,	512.0f,	700.0f,	-384.0f,	512.0f,	512.0f,	512.0f));

/*
		Rectangles.push_back(*new D3DRectangle(-30.0f,  22.5f,	-0.1f,	30.0f,	-22.5f,		-0.1f,	-0.1f,	-0.1f));*/
		D3DRectangle *robotsMonitor = new D3DRectangle(-30.0f,  22.5f,	-0.1f,	30.0f,	-22.5f,		-0.1f,	-0.1f,	-0.1f);

		robotsMonitor -> getRect() -> right = 0.625f;
		robotsMonitor -> getRect() -> bottom = 0.9375f;

		Rectangles.push_back(*robotsMonitor);



		Rectangles.push_back(*new D3DRectangle(32.0f,	22.5f,	-0.1f,	92.0f,	-22.5f,		-0.1f,	-0.1f,	-0.1f));		
		Rectangles.push_back(*new D3DRectangle(-92.0f,  22.5f,	-0.1f,	-32.0f,	-22.5f,		-0.1f,	-0.1f,	-0.1f));

		StaticTextures.clear();
		StaticTextures.resize(5);
		D3DXCreateTexture(pd3dDevice, 1, 1, 1, D3DUSAGE_DYNAMIC, D3DFMT_A8R8G8B8, D3DPOOL_DEFAULT, &StaticTextures[0]);
		D3DXCreateTextureFromFile(pd3dDevice, L"UI_textures/Background.jpg",    &StaticTextures[1]);
		D3DXCreateTextureFromFile(pd3dDevice, L"UI_textures/Robotino_logo.jpg", &StaticTextures[2]);
		D3DXCreateTextureFromFile(pd3dDevice, L"UI_textures/Robotino_logo2.png",&StaticTextures[3]);
		D3DXCreateTextureFromFile(pd3dDevice, L"UI_textures/Robotino_logo3.png",&StaticTextures[4]);

								//   x1  y1  z1 x2  y2  z2  z3  z4
		UI.push_back( new D3DXButton (	pd3dDevice, 
										static_f_b_Connect, 
										L"UI_textures/Connect_normal.png", 
										L"UI_textures/Connect_hover.png", 
										L"UI_textures/Connect_active.png", 
										40,-30,-10,70,-38,-10,-10,-10
									)
					);
		UI.push_back( new D3DXCheckButton_Thread(	pd3dDevice, 
													static_f_b_Line, 
													L"UI_textures/MovetoLine_normal.png", 
													L"UI_textures/MovetoLine_hover.png", 
													L"UI_textures/MovetoLine_active.png", 
												//   x1  y1  z1   x2  y2  z2  z3  z4
													15,-30.5,-10, 35,-37.5,-10,-10,-10
												)
					);
		UI.push_back( new D3DXScrollBar(	pd3dDevice, 
											static_f_v_Update_Speed, 
											L"UI_textures/ScrollBar_Line.png", 
											L"UI_textures/ScrollBar_normal.png", 
											L"UI_textures/ScrollBar_active.png",
											0.0,
											3,
											6,
											0.01,
										//   x1  y1   z1     x2    y2    z2  z3  z4
											90,  25, -5,    91, -25, -5, -5, -5
										)
					);

		UI.push_back( new D3DXScrollBar(	pd3dDevice, 
											static_f_v_Update_Alpha, 
											L"UI_textures/ScrollBar_Line.png", 
											L"UI_textures/ScrollBar_normal.png", 
											L"UI_textures/ScrollBar_active.png",
											0.11111f,
											2,
											4,
											1,
										//   x1    y1   z1   x2    y2    z2  z3  z4
											30.5,  23, -1,   31.5, -23, -1, -1, -1
										)
					);

		UI.push_back( new D3DXScrollBar(	pd3dDevice, 
											static_f_v_Update_Height, 
											L"UI_textures/ScrollBar_Line.png", 
											L"UI_textures/ScrollBar_normal.png", 
											L"UI_textures/ScrollBar_active.png",
											0.11111f,
											2,
											4,
											1,
										//   x1    y1   z1   x2    y2    z2  z3  z4
											-31.5, 23, -1,  -30.5, -23, -1, -1, -1
										)
					);


		//! Color Filter ===============================================================================

		UI.push_back( new D3DXScrollBar(	pd3dDevice, 
											static_f_v_Filter_R, 
											L"UI_textures/ScrollBar_Line.png", 
											L"UI_textures/ScrollBar_normal.png", 
											L"UI_textures/ScrollBar_active.png",
											0.0f,
											2,
											6,
											0.01,
										//   x1    y1   z1   x2    y2    z2  z3  z4
											43,  51, -1,  44,   21, -1, -1, -1
											
										)
					);
		UI.push_back( new D3DXScrollBar(	pd3dDevice, 
											static_f_v_Filter_G, 
											L"UI_textures/ScrollBar_Line.png", 
											L"UI_textures/ScrollBar_normal.png", 
											L"UI_textures/ScrollBar_active.png",
											0.0f,
											2,
											6,
											0.01,
										//   x1    y1   z1   x2    y2    z2  z3  z4
											57,  51, -1,  58,   21, -1, -1, -1
										)
					);
		UI.push_back( new D3DXScrollBar(	pd3dDevice, 
											static_f_v_Filter_B, 
											L"UI_textures/ScrollBar_Line.png", 
											L"UI_textures/ScrollBar_normal.png", 
											L"UI_textures/ScrollBar_active.png",
											0.0f,
											2,
											6,
											0.01,
										//   x1    y1   z1   x2    y2    z2  z3  z4
											71,   51, -1,  72,   21, -1, -1, -1
										)
					);
		UI.push_back( new D3DXScrollBar(	pd3dDevice, 
											static_f_v_Filter_Size, 
											L"UI_textures/ScrollBar_Line.png", 
											L"UI_textures/ScrollBar_normal.png", 
											L"UI_textures/ScrollBar_active.png",
											0.0f,
											2.5,
											5,
											0.01,
										//   x1    y1   z1   x2    y2    z2  z3  z4
											85,  51, -1,  86,   21, -1, -1, -1
										)
					);

		//! Speed Move to line regulator ===============================================================

		UI.push_back( new D3DXScrollBar(	pd3dDevice,
											static_f_v_SpeedRegulator,
											L"UI_textures/ScrollBar_Line.png",
											L"UI_textures/ScrollBar_normal.png",
											L"UI_textures/ScrollBar_active.png",
											0.5f,
											1.5f,
											4.0f,
											0.01f,
										//   x1    y1   z1   x2    y2    z2  z3  z4
											96,    32,  -5,  97,   -32,  -5, -5, -5
										)
					);


		Text.push_back(D3DXText(pd3dDevice, L"Blue"));
		Text.push_back(D3DXText(pd3dDevice, L"Blue"));
		Text.push_back(D3DXText(pd3dDevice, L"Blue"));
		Text.push_back(D3DXText(pd3dDevice, L"Blue"));
		Text.push_back(D3DXText(pd3dDevice, L"Blue"));

		SetupVB();
	}
	bool Render()
	{
		if(Text.size() == 0) return false;

		for(int q=0;q<Text.size();q++)
		{
			Text[q].SetX(Move(q%2==0,1200 + q*10,20,5));
			Text[q].SetY(62*q + 2);
		}

		WCHAR buf[512];
		_i64tow((int)Robo_Move.w, buf, 10);
		Text[0].Cpy(L"SPEED◄");		Text[0].Cat(buf);
		
		_i64tow((int)Robo_Cam_Rx, buf, 10);
		Text[1].Cpy(L"Alpha◄");		Text[1].Cat(buf);

		_i64tow((int)Robo_Cam_Height, buf, 10);
		Text[2].Cpy(L"Height◄");	Text[2].Cat(buf);
		
		_i64tow((int)y, buf, 10);
		Text[3].Cpy(L"Y◄");			Text[3].Cat(buf);

		_i64tow((int)x, buf, 10);
		Text[4].Cpy(L"X◄");			Text[4].Cat(buf);

		_i64tow((int)r, buf, 10);
		Text[5].Cpy(L"r◄");			Text[5].Cat(buf);

		int							green_blue	=	abs(Robo_Move.w) / 4 * 2.55f;

		Text[0].SetColor(D3DCOLOR_XRGB(255, green_blue, green_blue));
		green_blue	=	abs(Robo_Cam_Rx) / 0.9 * 2.55f;
		Text[1].SetColor(D3DCOLOR_XRGB(255, green_blue, green_blue));
		green_blue	=	abs(Robo_Cam_Height) / 50 * 2.55f;
		Text[2].SetColor(D3DCOLOR_XRGB(255, green_blue, green_blue));
		green_blue	=	abs(y) / 4 * 2.55f;
		Text[3].SetColor(D3DCOLOR_XRGB(255, green_blue, green_blue));
		green_blue	=	abs(x) / 4 * 2.55f;
		Text[4].SetColor(D3DCOLOR_XRGB(255, green_blue, green_blue));
		green_blue	=	abs(r) / 0.7 * 2.55f;
		Text[5].SetColor(D3DCOLOR_XRGB(255, green_blue, green_blue));

		Cam.Render(pd3dDevice, D3DCOLOR_ARGB(50,150, 100, 220), Lines);

		beginRengerGlobal();
			bool result = RenderGlobal(D3DCOLOR_XRGB(0, 0, 0));	
			Lasers.UpdateSensors(pd3dDevice, 4, 0.4f, 10);
			Lasers.Render(pd3dDevice);
		endRenderGlobal();

		return result;
	}

//! --------------------------------------------Robotino Interface Functions------------------------------------------*/
	void init(char *ip)
	{
		setAddress			(ip);
		omniDrive.setComId	(id());
		connect				(true);
		Lasers.Connect(id());
	}

	//! -------------------------------------------------Messages-------------------------------------------------*/
	void errorEvent( Error error,const char *errorString )
	{
		size_t	size	=	strlen(errorString);
		mbstowcs_s		(&size, &buf[0], 512, errorString, 512);

		MessageBox		(hWnd,buf,L"Error",MB_ICONERROR|MB_OK);
		SetWindowText	(hWnd,L"Robotino Disconnected");
	}
	void connectedEvent()
	{
		SetWindowText		(hWnd, L"Robotino Connected");
		Cam.setComId		(id());
		Cam.setStreaming	(true);
	}
	void connectionClosedEvent()
	{
		SetWindowText(hWnd,L"Robotino Disconnected");
	}

//! ---------------------------------------------Robotino Logical Functions-------------------------------------------*/

	void Move_to_line()
	{
		f_b_Line();
	}


	//---------------------------------------------Labirynth---------------------------------------------------------

	bool labyrinth(double speed)
	{
		//do

		// bool result = !bumper.value();
		// if(result)
		{
			double goLeft = 0.0;
			if(Lasers.buf[7] < 1.5 && Lasers.buf[7] > 0.35)
			{
				goLeft = -50.0;
			}
			else if (Lasers.buf[7] > 1.6)
			{
				goLeft = 50.0;
			}
			setVelocityForRot(speed, 0, goLeft);
			if(Lasers.buf[0] > 1.2 || Lasers.buf[8] > 1.8)
			{
				waitForUpdate();
				setVelocityForRot(0, 0, 0);
				rotateOld(60, 90);
			}
			if(Lasers.buf[7] < 0.35 && Lasers.buf[7] != 0)
			{
				labyrinthLostWallOld(speed);
			}
		}//while(!bumper.value());
		//setVelocity(0,0,0,0);
		return true;
	}

	bool rotate(double speed, double rot)
	{
		if ( isSecTime )
			return true;
		if ( !isDoingSmthng )
		{
			isDoingSmthng = true;
			setVelocityForRot(0,0,0);
			waitForUpdate();
			odometry.set(0,0,0);
			 int spin = (rot > 0)? 1: -1;
			 rot += spin*abs(rot)*13.0/180.0;
			 float phi = 0;
			 if (phi >= 0 && (phi + rot) > 180.0)
			 {
				rot = phi + rot - 360.0;
			 }
			 else if ((phi + rot) < -180.0)
			 {
				rot = phi + rot + 360.0;
			 }
			 else
			 {
				rot += phi;
			 }
			 setVelocityForRot(0,0,spin*speed);
		}
		waitForUpdate();
		int sign = (odometry.phi() - rot < 0)? -1: 1;

		if ( abs( odometry.phi() - rot ) >= 0.9 )/*sign == ((odometry.phi() - rot < 0)? -1: 1) && abs(odometry.phi() - rot) > 0.9 )*/
		{
			isSecTime = true;
			setVelocityForRot(0, 0, 0);
			isDoingSmthng = false;
			//waitForUpdate();
		}
		
		return true;
	}

	void rotateOld(double speed, double rot)
	{
		setVelocity(0,0,0,0);
		waitForUpdate();
		 odometry.set(0,0,0);
		 waitForUpdate();
		 int spin = (rot > 0)? 1: -1;
		 rot += spin*abs(rot)*13.0/180.0;
		 float phi = 0;
		 if (phi >= 0 && (phi + rot) > 180.0)
		 {
			rot = phi + rot - 360.0;
		 }
		 else if ((phi + rot) < -180.0)
		 {
			rot = phi + rot + 360.0;
		 }
		 else
		 {
			rot += phi;
		 }
		 setVelocity(0,0,spin*speed, Speed_Auto_Move);
		 int sign = (odometry.phi() - rot < 0)? -1: 1;
		 do {
			 setVelocityForRot(0,0,spin*speed);
			 waitForUpdate();
		 } while (sign == ((odometry.phi() - rot < 0)? -1: 1) && abs(odometry.phi() - rot) > 0.9);
		 setVelocityForRot(0,0,0);
		 waitForUpdate();

	}

	bool labyrinthLostWall(double speed)
	{
		if ( isSecTime )
			return true;
		
		switch( cur_lost_pos )
		{
		case 0:
			cur_lost_pos = 1;
			odometry.set(0,0,0);
			waitForUpdate();
			setVelocityForRot( (speed - 50), 0, 0 );
			break;
		case 1:
			if ( odometry.x() >= 280 )
			{
				cur_lost_pos = 2;
				setVelocityForRot( 0, 0, 0 );
				odometry.set(0, 0, 0);
				waitForUpdate();
				setVelocityForRot( 0, 0, -60 );
			}
			break;
		case 2:
			if ( abs( odometry.phi() ) >= 90 )
			{
				cur_lost_pos = 3;
				setVelocityForRot( 0, 0, 0 );
				odometry.set( 0, 0, 0 );
				waitForUpdate();
				setVelocityForRot( (speed - 50), 0, 0 );
			}
			break;
		case 3:
			if ( odometry.x() >= 200 )
			{
				cur_lost_pos = 0;
				isSecTime = true;
				setVelocityForRot( 0, 0, 0 );
			}
			break;
		}

		return true;
	}

	void labyrinthLostWallOld(double speed)
	{
		odometry.set(0,0,0);
		waitForUpdate();
		do{
			setVelocityForRot(0,(speed - 50),0);
		} while (odometry.x() < 280);
		setVelocity(0,0,0,0);
		odometry.set(0,0,0);
		do {
			setVelocity(0,0, -60, Speed_Auto_Move);
		} while (abs(odometry.phi()) < 90);
		setVelocityForRot(0,0,0);
		odometry.set(0,0,0);
		waitForUpdate();
		do{
			setVelocityForRot(0,(speed - 50),0);
		}while (odometry.x() < 200);
		setVelocityForRot(0,0,0);
	}

	bool goWhileSpace(double speed)
	{
		waitForUpdate();
		do{
			setVelocity(0, speed, 0, Speed_Auto_Move);
		}while(!bumper.value());
		setVelocity(0, 0, 0, 0);
		return true;
	}
	
	bool goDistance(double speed, double santimetres)
	{
		if ( isSecTime )
			return true;
		if ( !isDoingSmthng )
		{
			setVelocity(0,0,0,0);
			odometry.set(0,0,0);
			waitForUpdate();
			setVelocity(0, speed, 0, Speed_Auto_Move);
			isDoingSmthng = true;
		}

		if ( odometry.x() >= santimetres * 10 )
		{
			setVelocity(0, 0, 0, 0);
			isDoingSmthng = false;
			isSecTime = true;
		}
		
		return true;

		/*if ( isDoingSmthng )
			return true;

		isDoingSmthng = true;
		setVelocity(0,0,0,0);
		odometry.set(0,0,0);
		waitForUpdate();
		do{
			setVelocity(0, speed, 0, Speed_Auto_Move);
		}while(odometry.x() < santimetres * 10);
		setVelocity(0,0,0,0);
		//isDoingSmthng = false;
		return true;*/
	}

	bool goSide(double speed, double santimetres)
	{
		if ( isSecTime )
			return true;
		if ( !isDoingSmthng )
		{
			setVelocity(0,0,0,0);
			odometry.set(0,0,0);
			waitForUpdate();
			setVelocity(speed, 0, 0, Speed_Auto_Move);
			isDoingSmthng = true;
		}

		if ( odometry.y() >= santimetres * 10 )
		{
			setVelocity(0, 0, 0, 0);
			isDoingSmthng = false;
			isSecTime = true;
		}
		
		return true;
		/*setVelocity(0,0,0,0);
		odometry.set(0,0,0);
		waitForUpdate();
		do
		{
			setVelocity(speed, 0, 0, Speed_Auto_Move);
		}while(abs(odometry.y()) < abs(santimetres * 10));
			setVelocity(0,0,0,0);
		return true;*/
	}

	bool arc(double speed, double radius, double angle, int direction)
	{
		if ( isSecTime )
			return true;

		angle = angle * M_PI / 180;
		int quarter = 4;
		if(angle < (M_PI / 2)) quarter = 1;
		else if (angle < M_PI) quarter = 2;
			 else if (angle < (3 * M_PI / 2)) quarter = 3;
		double si = sin(angle);
		double co = cos(angle);
		radius *= 1e+4;
		double tm = radius * 2.0 * M_PI / speed;

		if ( !isDoingSmthng )
		{
			isDoingSmthng = true;
			odometry.set( 0, 0, 0 );
			timer.start();
		}
		double rot = timer.msecsElapsed() * 2 * M_PI / tm;
		switch(direction)
		{
			case 1: setVelocityForRot(sin(rot) * speed, (-1) * cos(rot) * speed, 0); break;//right_forward
			case 2: setVelocityForRot(sin(rot) * speed, cos(rot) * speed, 0); break;//left_forward
			case 3: setVelocityForRot((-1) * sin(rot) * speed, (-1) * cos(rot) * speed, 0); break;//right_back
			case 4: setVelocityForRot((-1) * sin(rot) * speed, cos(rot) * speed, 0); break;//left_back
		}

		bool flag = false;

		if(quarter == 1)
		{
			if((sin(rot) > si) && (cos(rot) < co)) flag = true;
		}
		else if(quarter == 2)
		{
			if((sin(rot) < si) && (cos(rot) < co)) flag = true;
		}
		else if(quarter == 3)
		{
			if((sin(rot) < si) && (cos(rot) > co)) flag = true;
		}
		else if((sin(rot) > si) && (cos(rot) > co) && (sin(rot) < 0.0)) flag = true;

		if ( flag )
		{
			isSecTime = true;
			timer.reset();
			setVelocityForRot( 0, 0, 0 );
			isDoingSmthng = false;
		}

		return true;


		/*
		odometry.set(0,0,0);
		angle = angle * M_PI / 180;
		int quarter = 4;
		if(angle < (M_PI / 2)) quarter = 1;
		else if (angle < M_PI) quarter = 2;
			 else if (angle < (3 * M_PI / 2)) quarter = 3;
		double si = sin(angle);
		double co = cos(angle);
		rec::core_lt::Timer timer;
		radius *= 1e+4;
		double tm = radius * 2.0 * M_PI / speed;
		double rot = 0;
		timer.start();
		for(;;)
		{
			rot = timer.msecsElapsed() * 2 * M_PI / tm;
			switch(direction)
			{
				case 1: setVelocity((-1) * cos(rot) * speed, sin(rot) * speed, 0, Speed_Auto_Move); break;//right_forward
				case 2: setVelocity(cos(rot) * speed, sin(rot) * speed, 0, Speed_Auto_Move); break;//left_forward
				case 3: setVelocity((-1) * cos(rot) * speed, (-1) * sin(rot) * speed, 0, Speed_Auto_Move); break;//right_back
				case 4: setVelocity(cos(rot) * speed, (-1) * sin(rot) * speed, 0, Speed_Auto_Move); break;//left_back
			}
			
			if(quarter == 1)
			{
				if((sin(rot) > si) && (cos(rot) < co)) break;
			}
			else if(quarter == 2)
			{
				if((sin(rot) < si) && (cos(rot) < co)) break;
			}
			else if(quarter == 3)
			{
				if((sin(rot) < si) && (cos(rot) > co)) break;
			}
			else if((sin(rot) > si) && (cos(rot) > co) && (sin(rot) < 0.0)) break;
		}
		setVelocity(0,0,0,0);

		return true;*/
	}

	bool obsFollow(double speed, int sensor)
	{
		do
		{
			double goLeft = 0.0;
			if(Lasers.buf[sensor] < 1.5 && Lasers.buf[sensor] > 1.0)
			{
				goLeft = -40.0;
			}
			else if (Lasers.buf[sensor] > 1.8)
			{
				goLeft = 40.0;
			}
			setVelocity(0, speed, goLeft,Speed_Auto_Move);
		}while(Lasers.buf[sensor] > 0.95);
		setVelocity(0,0,0,0);
		return true;
	}
	
};